﻿#include "cydevice.h"
#include <QDebug>

CyDevice::CyDevice()
	: device(nullptr),
	  bitwidth(8),
	  bufferIndex(0)
{
}

CyDevice::~CyDevice()
{
	for (auto& image : imageBuffer) {
		delete[] image.data;
	}
}

void CyDevice::setCommandSet(CameraConfig::Command command)
{
	this->command = std::move(command);
	resize(command.maxSize.width() * command.maxSize.height() * 2, command.queueSize);
}

bool CyDevice::open(HANDLE hnd)
{
	if (device != nullptr) return false;
	device = new CCyUSBDevice(hnd);
	if (!device->Open(0)) {
		delete device;
		device = nullptr;
		return false;
	}
	dataInEndPoint = device->BulkInEndPt;
	controlEndPoint = device->ControlEndPt;

	sendControlCode(0xb2); //usb设备初始化

	mutex.lockForRead();
	sendControlCode(bitwidth == 8 ? 0xb8 : 0xb9);
	mutex.unlock();

	sendControlCode(0xb4); //清空设备帧缓存

	bufferIndex = 0;
	return true;
}

void CyDevice::close()
{
	if (device != nullptr) {
		delete device;
		device = nullptr;
	}
}

void CyDevice::resize(int maxSize, int queueSize)
{
	// 删除之前分配的空间
	for (auto& image : imageBuffer) {
		delete[] image.data;
	}

	imageBuffer.resize(queueSize);
	for (int i = 0; i < queueSize; ++i) {
		imageBuffer[i].data = new uchar[maxSize];
	}
}

bool CyDevice::empty()
{
	return imageQueue.empty();
}

ImageData CyDevice::read()
{
	auto image = imageQueue.head();
	ImageData retImage = image->copy();
	imageQueue.dequeue();
	return retImage;
}

void CyDevice::enableReceive(bool enable)
{
	mutex.lockForWrite();
	isReceiving = enable;
	mutex.unlock();
}

bool CyDevice::sendControlCode(int code)
{
	controlEndPoint->Target = TGT_DEVICE;
	controlEndPoint->ReqType = REQ_VENDOR;
	controlEndPoint->Direction = DIR_TO_DEVICE;
	controlEndPoint->Value = 0;
	controlEndPoint->Index = 0;
	controlEndPoint->ReqCode = code;

	UCHAR buf = 0;
	LONG len = 0;
	return controlEndPoint->XferData(&buf, len);
}

bool CyDevice::sendRequestCode(int code, QVector<uchar> buf)
{
	controlEndPoint->Target = TGT_DEVICE;
	controlEndPoint->ReqType = REQ_VENDOR;
	controlEndPoint->Direction = DIR_TO_DEVICE;
	controlEndPoint->Value = 0;
	controlEndPoint->Index = 0;
	controlEndPoint->ReqCode = code;

	LONG len = buf.size();
	return controlEndPoint->XferData(buf.data(), len);
}

void CyDevice::receive(LONG sizePerXfer, int xferQueueSize, int timeOut)
{
	OVERLAPPED* inOverlaps = new OVERLAPPED[xferQueueSize];
	UCHAR** inContexts = new UCHAR*[xferQueueSize];

	for (int i = 0; i < xferQueueSize; ++i) {
		inOverlaps[i].hEvent = CreateEvent(nullptr, false, false, L"CYUSB");
	}

	while (true) {
		mutex.lockForRead();
		if (!isReceiving) {
			mutex.unlock();
			break;
		}

		uchar* data = imageBuffer[bufferIndex].data;
		imageBuffer[bufferIndex].bitwidth = bitwidth;
		imageBuffer[bufferIndex].height = height;
		imageBuffer[bufferIndex].width = width;
		LONG size = bitwidth > 8 ? width * height * 2 : width * height;

		mutex.unlock();

		sendControlCode(0xb5);

		int transferCount = 0;
		LONG sizePerDataXfer = sizePerXfer;
		int transferCountLimit = size / sizePerDataXfer;

		for (int i = 0; i < xferQueueSize; ++i) {
			inContexts[i] = dataInEndPoint->BeginDataXfer(data + i * sizePerDataXfer, sizePerDataXfer, &inOverlaps[i]);
			if (dataInEndPoint->NtStatus || dataInEndPoint->UsbdStatus) {
				qDebug() << "BeginDataXfer failed!";
				return;
			}
		}

		int transferQueueCount = xferQueueSize;
		int index = 0;
		int count = 0;
		while (true) {
			if (!dataInEndPoint->WaitForXfer(&inOverlaps[index], timeOut)) {
				dataInEndPoint->Abort();
				qDebug() << "WaitForXfer failed!" << index << "  " << transferCount;
				break;
			}

			if (!dataInEndPoint->FinishDataXfer(data + transferCount * sizePerDataXfer,
			                                    sizePerDataXfer, &inOverlaps[index], inContexts[index])) {
				qDebug() << "FinishDataXfer failed!";
				break;
			}
			++count;
			++transferCount;

			if (transferQueueCount < transferCountLimit) {
				inContexts[index] = dataInEndPoint->BeginDataXfer(data + transferQueueCount * sizePerDataXfer,
				                                                  sizePerDataXfer, &inOverlaps[index]);
				if (dataInEndPoint->NtStatus || dataInEndPoint->UsbdStatus) {
					qDebug() << "BeginDataXfer in loop failed!";
					return;
				}
				++transferQueueCount;
			}

			++index;

			if (index == xferQueueSize)
				index = 0;

			if (transferCount == transferCountLimit) {
				if (imageQueue.size() < imageBuffer.size()) {
					imageQueue.enqueue(&imageBuffer[bufferIndex]);
					emit received();
					++bufferIndex;
					if (bufferIndex == imageBuffer.size())
						bufferIndex = 0;
				}
				break;
			}
		}
	}

	for (int i = 0; i < xferQueueSize; ++i) {
		CloseHandle(inOverlaps[i].hEvent);
	}

	delete[] inOverlaps;
	delete[] inContexts;
}

bool CyDevice::receiving()
{
	mutex.lockForRead();
	bool receving = isReceiving;
	mutex.unlock();
	return receving;
}

void CyDevice::wait()
{
	while (true) {
		mutex.tryLockForRead(-1);
		if (isReceiving) {
			mutex.unlock();
			break;
		}
		mutex.unlock();
	}
	qDebug() << "wait change done";
	imageQueue.clear();
}

void CyDevice::setResolution(int width)
{
	auto& res = command.resolution[QString::number(width)];
	
	if (sendControlCode(res["req"])) {
		this->width = width;
		this->height = res["height"];
	}
	mutex.lockForWrite();
	isReceiving = true;
	mutex.unlock();

	receive(res["sizePerXfer"], res["queueSize"], res["timeout"]);
}

void CyDevice::setBitwidth(int bitwidth)
{
	sendControlCode(command.bitwidth[bitwidth]);
	mutex.lockForWrite();
	this->bitwidth = bitwidth;
	mutex.unlock();
}

void CyDevice::setAnalog(int analog)
{
	auto gain = command.analog[analog];
	for (auto& addr : command.analogAddress) {
		sendRequestCode(0xb3, {addr.first, addr.second, 0x00, gain});
	}
	if (analog >= 4) {
		sendRequestCode(0xb3, command.analogExtraCode);
	}
}

void CyDevice::setExposure(int exposure)
{
	const double k = command.exposure[QString::number(width)][bitwidth] * 1000.0 / (width + 2);
	const double b = 426.0 / (width + 2);
	uint code = static_cast<int>(k * exposure + b);
	uchar hi = (code >> 8) & 0xff, lo = code & 0xff;
	sendRequestCode(0xb3, {command.exposureAddress.first, command.exposureAddress.second, hi, lo});
}

